#ifndef __ASCTEC_LL_DATA_STRUCTURES_H__
#define __ASCTEC_LL_DATA_STRUCTURES_H__

#ifdef __cplusplus
extern "C" {
#endif

/*
 * Info from:
 * http://www.asctec.de/downloads/manuals/AscTec-Autopilot-Manual-v10.pdf
 *
 * Data structures
 * LL_STATUS: Status information of the low level processor
 * IMU_RAWDATA: Sensor raw-values
 * IMU_CALCDATA: Calibrated sensor outputs and data fusion results
 * CTRL_INTERNALOUT: Controller internals and outputs before output mapping
 * CTRL_OUT: Controller outputs after output mapping
 * RC_Data: Data received by the R/C receiver
 * GPS_Data: GPS data
 * GPS_Data Advanced: GPS data + position and speed estimates after data fusion
 *
 * All structures are sent as follows:
 *   startstring: >*>
 *   length of structure: unsigned short
 *   packet descriptor: unsigned char
 *   the actual data structure: ”length of structure” bytes
 *   crc16: unsigned short
 *   stopstring: <#<
 */

static const int asctec_packet_startstring_size = 3;
static const char asctec_packet_startstring[3] = { '>', '*', '>' };

static const int asctec_packet_stopstring_size = 3;
static const char asctec_packet_stopstring[3] = { '>', '*', '>' };

static const int min_packet_size = 12;  //length of packet with 1 byte payload
static const int max_packet_size = 128;  //imu_calcdata is the biggest at 103

//packet descriptors
#define ASCTEC_PD_IMURAWDATA 0x01
#define ASCTEC_PD_LLSTATUS 0x02
#define ASCTEC_PD_IMUCALCDATA 0x03
#define ASCTEC_PD_HLSTATUS 0x04
#define ASCTEC_PD_DEBUGDATA 0x05
#define ASCTEC_PD_CTRLOUT 0x11
#define ASCTEC_PD_FLIGHTPARAMS 0x12
#define ASCTEC_PD_CTRLCOMMANDS 0x13
#define ASCTEC_PD_CTRLINTERNAL 0x14
#define ASCTEC_PD_RCDATA 0x15
#define ASCTEC_PD_CTRLSTATUS 0x16
#define ASCTEC_PD_CTRLINPUT 0x17
#define ASCTEC_PD_CTRLFALCON 0x18
#define ASCTEC_PD_WAYPOINT 0x20
#define ASCTEC_PD_CURRENTWAY 0x21
#define ASCTEC_PD_NMEADATA 0x22
#define ASCTEC_PD_GPSDATA 0x23
#define ASCTEC_PD_SINGLEWAYPOINT 0x24
#define ASCTEC_PD_GOTOCOMMAND 0x25
#define ASCTEC_PD_LAUNCHCOMMAND 0x26
#define ASCTEC_PD_LANDCOMMAND 0x27
#define ASCTEC_PD_HOMECOMMAND 0x28
#define ASCTEC_PD_GPSDATAADVANCED 0x29

//Flight Modes
#define ASCTEC_FM_ACC      0x01
#define ASCTEC_FM_HEIGHT      0x02
#define ASCTEC_FM_POS      0x04
#define ASCTEC_FM_SPEED       0x08
#define ASCTEC_FM_WP          0x10
#define ASCTEC_FM_SCIENTIFIC    0x20
#define ASCTEC_FM_SCIENTIFIC_ACTIVE 0x40
#define ASCTEC_FM_EMERGENCY   0x80
#define ASCTEC_FM_CALIBRATION_ERROR 0x100

// Request Bits for polling data structures
#define ASCTEC_REQUEST_LL_STATUS_BIT          0x0001
#define ASCTEC_REQUEST_IMU_RAWDATA_BIT        0x0002
#define ASCTEC_REQUEST_IMU_CALCDATA_BIT       0x0004
#define ASCTEC_REQUEST_RC_DATA_BIT            0x0008
#define ASCTEC_REQUEST_CTRL_OUT_BIT           0x0010
#define ASCTEC_REQUEST_GPS_DATA_BIT           0x0080
#define ASCTEC_REQUEST_CURRENT_WAY_BIT        0x0100
#define ASCTEC_REQUEST_GPS_DATA_ADVANCED_BIT  0x0200
#define ASCTEC_REQUEST_CAM_DATA_BIT           0x0800

inline unsigned short asctec_crc_update(unsigned short crc,
                                        unsigned char data) {
  data ^= (crc & 0xff);
  data ^= data << 4;
  return ((((unsigned short) data << 8) | ((crc >> 8) & 0xff))
      ^ (unsigned char) (data >> 4) ^ ((unsigned short) data << 3));
}

inline unsigned short asctec_crc16(const void* data, unsigned short cnt) {
  unsigned short crc = 0xff;
  unsigned char * ptr = (unsigned char *) data;
  int i;
  for (i = 0; i < cnt; i++) {
    crc = asctec_crc_update(crc, *ptr);
    ptr++;
  }
  return crc;
}

typedef struct {
  //battery voltages in mV
  short battery_voltage_1;
  short battery_voltage_2;
  //don't care
  short status;
  //Controller cycles per second (should be ~1000)
  short cpu_load;
  //don't care
  char compass_enabled;
  char chksum_error;
  char flying;
  char motors_on;
  short flightMode;
  //Time since system was turned on
  short up_time;
} asctec_ll_status_t;

typedef struct {
  //pressure sensor 24-bit value, not scaled but bias free
  int pressure;
  //16-bit gyro readings; 32768 = 2.5V
  short gyro_x;
  short gyro_y;
  short gyro_z;
  //10-bit magnetic field sensor readings
  short mag_x;
  short mag_y;
  short mag_z;
  //16-bit accelerometer readings
  short acc_x;
  short acc_y;
  short acc_z;
  //16-bit temperature measurement using yaw-gyro internal sensor;
  unsigned short temp;
  //16-bit temperature measurement using ADC internal sensor
  unsigned int temp_ADC;
} asctec_imu_rawdata_t;

typedef struct {
  //angles derived by integration of gyro_outputs, drift compensated by data fusion;
  //-90000..+90000 pitch(nick) and roll, 0..360000 yaw; 1000 = 1 degree
  int angle_nick;
  int angle_roll;
  int angle_yaw;
  //angular velocities, raw values [16 bit] but bias free
  int angvel_nick;
  int angvel_roll;
  int angvel_yaw;
  //acc-sensor outputs, calibrated: -10000..+10000 = -1g..+1g
  short acc_x_calib;
  short acc_y_calib;
  short acc_z_calib;
  //horizontal / vertical accelerations: -10000..+10000 = -1g..+1g
  short acc_x;
  short acc_y;
  short acc_z;
  //reference angles derived by accelerations only: -90000..+90000; 1000 = 1 degree
  int acc_angle_nick;
  int acc_angle_roll;
  //total acceleration measured (10000 = 1g)
  int acc_absolute_value;
  //magnetic field sensors output, offset free and scaled; units not determined, as only the direction of the field vector is taken into account
  int Hx;
  int Hy;
  int Hz;
  //compass reading: angle reference for angle_yaw: 0..360000; 1000 = 1 degree
  int mag_heading;
  //pseudo speed measurements: integrated accelerations, pulled towards zero; units unknown; used for short-term position stabilization
  int speed_x;
  int speed_y;
  int speed_z;
  //height in mm (after data fusion)
  int height;
  //diff. height in mm/s (after data fusion)
  int dheight;
  //diff. height measured by the pressure sensor [mm/s]
  int dheight_reference;
  //height measured by the pressure sensor [mm]
  int height_reference;
} asctec_imu_calcdata_t;

typedef struct {
//latitude/longitude in deg * 10ˆ7
  int latitude;
  int longitude;
//GPS height in mm
  int height;
//speed in x (E/W) and y(N/S) in mm/s
  int speed_x;
  int speed_y;
//GPS heading in deg * 1000
  int heading;
//accuracy estimates in mm and mm/s
  unsigned int horizontal_accuracy;
  unsigned int vertical_accuracy;
  unsigned int speed_accuracy;
//number of satellite vehicles used in NAV solution
  unsigned int numSV;
// GPS status information; 0x03 = valid GPS fix
  int status;
} asctec_gps_data_t;

typedef struct {
  //latitude/longitude in deg * 10ˆ7
  int latitude;
  int longitude;
//GPS height in mm
  int height;
//speed in x (E/W) and y(N/S) in mm/s
  int speed_x;
  int speed_y;
//GPS heading in deg * 1000
  int heading;
//accuracy estimates in mm and mm/s
  unsigned int horizontal_accuracy;
  unsigned int vertical_accuracy;
  unsigned int speed_accuracy;
//number of satellite vehicles used in NAV solution
  unsigned int numSV;
//GPS status information; 0x03 = valid GPS fix
  int status;
//coordinates of current origin in deg * 10ˆ7
  int latitude_best_estimate;
  int longitude_best_estimate;
//velocities in X (E/W) and Y (N/S) after data fusion
  int speed_x_best_estimate;
  int speed_y_best_estimate;
} asctec_gps_data_advanced_t;

typedef struct {
  //channels as read from R/C receiver
  unsigned short channels_in[8];
  //channels bias free, remapped and scaled to 0..4095
  unsigned short channels_out[8];
  //Indicator for valid R/C receiption
  unsigned int lock;
} asctec_rc_rawdata_t;

typedef struct {
  //attitude controller outputs; 0..200 = -100 ..+100%
  int nick;
  int roll;
  int yaw;
  //current thrust (height controller output); 0..200 = 0..100%
  int thrust;
} asctec_controller_output_t;

#ifdef __cplusplus
}
#endif

#endif //__ASCTEC_LL_DATA_STRUCTURES_H__
